import math

import numpy as np
import matplotlib.pyplot as plt
from math import pi, sin, cos, acos, atan2, sqrt
def Dubins_path(Pi, Pf, alpha, beta, ratio, tp="RSR", show_path=True):
    assert len(Pi) == 2 and len(Pf) == 2
    assert tp in ["RSR", "LSL", "RSL", "LSR"]
    assert ratio > 0.

    # 坐标变换
    Vec_Move = np.matrix([[1., 0., -Pi[0]], [0., 1., -Pi[1]], [0., 0., 1.]])  # 平移
    temp_theta = -(atan2(Pf[1] - Pi[1], Pf[0] - Pi[0]) + pi)
    alpha_ = alpha + temp_theta
    beta_ = beta + temp_theta
    Vec_Rotate = np.matrix([[cos(temp_theta), -sin(temp_theta), 0.],
                            [sin(temp_theta), cos(temp_theta), 0.],
                            [0., 0., 1.]])  # 旋转
    Vec_Length = np.matrix([[1. / ratio, 0., 0.],
                            [0., 1. / ratio, 0.],
                            [0., 0., 1.]])  # 拉伸
    Vec_Trans = Vec_Length * Vec_Rotate * Vec_Move  # 变换矩阵

    temp_res = Vec_Trans * [[Pi[0]], [Pi[1]], [1]]
    Pi_ = [temp_res[0, 0], temp_res[1, 0]]
    temp_res = Vec_Trans * [[Pf[0]], [Pf[1]], [1]]
    Pf_ = [temp_res[0, 0], temp_res[1, 0]]

    # 曲线计算
    if tp == "RSR":  # 双逆时针路线
        O1_x, O1_y = sin(alpha_), -cos(alpha_)          # 起点圆
        O2_x, O2_y = Pf_[0] + sin(beta_), -cos(beta_)   # 终点圆
        theta = atan2(O2_y - O1_y, O2_x - O1_x)         # 圆心夹角
        pho1 = (theta - alpha_) % (2.0 * pi)            # 起点圆弧度
        pho2 = sqrt((O1_x - O2_x)**2 + (O1_y - O2_y)**2)    # 直线长度
        pho3 = (beta_ - pho1 - pho2 - alpha_) % (2.0 * pi)  # 终点圆弧度
        A_x, A_y = sin(alpha_) - sin(alpha_ + pho1), -cos(alpha_) + cos(alpha_ + pho1)          # 切出点
        B_x, B_y = Pf_[0] + sin(beta_) - sin(alpha_ + pho1), -cos(beta_) + cos(alpha_ + pho1)   # 切入点
    elif tp == "LSL":  # 双顺时针路线
        O1_x, O1_y = -sin(alpha_), cos(alpha_)          # 起点圆
        O2_x, O2_y = Pf_[0] - sin(beta_), cos(beta_)    # 终点圆
        theta = atan2(O2_y - O1_y, O2_x - O1_x)         # 圆心夹角
        pho1 = (theta - alpha_) % (2.0 * pi)            # 起点圆弧度
        pho2 = sqrt((O1_x - O2_x)**2 + (O1_y - O2_y)**2)    # 直线长度
        pho3 = (beta_ - pho1 - pho2 - alpha_) % (2.0 * pi)  # 终点圆弧度
        A_x, A_y = -sin(alpha_) + sin(alpha_ + pho1), cos(alpha_) - cos(alpha_ + pho1)          # 切出点
        B_x, B_y = Pf_[0] - sin(beta_) + sin(alpha_ + pho1), cos(beta_) - cos(alpha_ + pho1)    # 切入点
    elif tp == "RSL":  # 八字交叉路线
        O1_x, O1_y = sin(alpha_), -cos(alpha_)          # 起点圆
        O2_x, O2_y = Pf_[0] - sin(beta_), cos(beta_)    # 终点圆
        theta = atan2(O2_y - O1_y, O2_x - O1_x)         # 圆心夹角
        dis_O1O2 = sqrt((O2_y - O1_y)**2 + (O2_x - O1_x)**2)
        if dis_O1O2 < 2.0:
            # print("The geometry constraint is not satisfied!")
            return [],[],-1
        else:
            theta_O1A = acos(2. / dis_O1O2)             # 圆1圆心和A夹角
            A_x, A_y = sin(alpha_) + cos(theta + theta_O1A), -cos(alpha_) + sin(theta + theta_O1A)          # 切出点
            B_x, B_y = Pf_[0] - sin(beta_) - cos(theta + theta_O1A), cos(beta_) - sin( theta + theta_O1A)   # 切入点
    else:  # "LSR" # 另一种八字交叉路线
        O1_x, O1_y = -sin(alpha_), cos(alpha_)          # 起点圆
        O2_x, O2_y = Pf_[0] + sin(beta_), -cos(beta_)   # 终点圆
        theta = atan2(O2_y - O1_y, O2_x - O1_x)         # 圆心夹角
        dis_O1O2 = sqrt((O2_y - O1_y)**2 + (O2_x - O1_x)**2)
        if dis_O1O2 < 2.0:
            # print("The geometry constraint is not satisfied!")
            return [],[],-1
        else:
            theta_O1A = acos(2. / dis_O1O2)             # 圆1圆心和A夹角
            A_x, A_y = -sin(alpha_) + cos(theta - theta_O1A), cos(alpha_) + sin(theta - theta_O1A)          # 切出点
            B_x, B_y = Pf_[0] + sin(beta_) - cos(theta - theta_O1A), -cos(beta_) - sin(theta - theta_O1A)   # 切入点

    # 逆变换
    Vec_Trans_INV = Vec_Trans.I
    temp_res = Vec_Trans_INV * [[A_x], [A_y], [1]]
    A_x, A_y = (temp_res[0, 0], temp_res[1, 0])
    temp_res = Vec_Trans_INV * [[B_x], [B_y], [1]]
    B_x, B_y = (temp_res[0, 0], temp_res[1, 0])
    temp_res = Vec_Trans_INV * [[O1_x], [O1_y], [1]]
    O1_x, O1_y = (temp_res[0, 0], temp_res[1, 0])
    temp_res = Vec_Trans_INV * [[O2_x], [O2_y], [1]]
    O2_x, O2_y = (temp_res[0, 0], temp_res[1, 0])

    dis_lst = []  # 每一段的距离
    ati1, ati2 = atan2(Pi[1] - O1_y, Pi[0] - O1_x), atan2(A_y - O1_y, A_x - O1_x)
    if tp[0] == "R" and ati1 < ati2:
        ati1 += 2.0 * pi
    elif tp[0] == "L" and ati1 > ati2:
        ati1 -= 2.0 * pi
    dis_lst.append(ratio * abs(ati1 - ati2))  # 切出弧长度
    dis_lst.append(sqrt((A_x - B_x)**2 + (A_y - B_y)**2))  # 直线段距离
    atf1 = atan2(Pf[1] - O2_y, Pf[0] - O2_x)
    atf2 = atan2(B_y - O2_y, B_x - O2_x)
    if tp[2] == "R" and atf2 < atf1:
        atf2 += 2.0 * pi
    elif tp[2] == "L" and atf2 > atf1:
        atf2 -= 2.0 * pi
    dis_lst.append(ratio * abs(atf1 - atf2))  # 切入弧长度
    if show_path:
        plt.figure(figsize=(12, 12))
        arrow_length = 17.5
        # 出发向量
        plt.arrow(Pi[0],
                  Pi[1],
                  arrow_length * cos(alpha),
                  arrow_length * sin(alpha),
                  width=0.01,
                  length_includes_head=True,
                  head_width=0.05,
                  head_length=0.1,
                  fc='b',
                  ec='b')
        # 到达向量
        plt.arrow(Pf[0],
                  Pf[1],
                  arrow_length * cos(beta),
                  arrow_length * sin(beta),
                  width=0.01,
                  length_includes_head=True,
                  head_width=0.05,
                  head_length=0.1,
                  fc='r',
                  ec='r')
        plt.scatter(np.array([Pi[0], A_x, B_x, Pf[0]]), np.array([Pi[1], A_y, B_y, Pf[1]]))  # 起止点,切点
        # plt.scatter(np.array([O1_x, O2_x]), np.array([O1_y, O2_y]), c='r')  # 圆心
        # the = np.linspace(-pi, pi, 200)  # 完整圆形轮廓
        # plt.scatter((O1_x + ratio * np.cos(the)), (O1_y + ratio * np.sin(the)), s=0.1, c='r')
        # plt.scatter((O2_x + ratio * np.cos(the)), (O2_y + ratio * np.sin(the)), s=0.1, c='r')

        # 出发圆弧轨迹
        the = np.linspace(ati2, ati1, 200)
        res_path_x = []
        res_path_y = []
        res_path_x = list((O1_x + ratio * np.cos(the)))
        res_path_y = list((O1_y + ratio * np.sin(the)))
        if math.sqrt((res_path_x[0] - Pi[0])**2 + (res_path_y[0] - Pi[1])**2) > math.sqrt((res_path_x[-1] - Pi[0])**2 + (res_path_y[-1] - Pi[1])**2):
            res_path_x.reverse()
            res_path_y.reverse()
        # print(len(res_path_x))
        plt.plot((O1_x + ratio * np.cos(the)), (O1_y + ratio * np.sin(the)), c='b')
        # plt.plot(res_path_x, res_path_y, c='b')
        # 直线段
        res_path_x.append(A_x)
        res_path_x.append(B_x)
        res_path_y.append(A_y)
        res_path_y.append(B_y)
        plt.plot([A_x, B_x], [A_y, B_y], c='b')

        # 到达圆弧轨迹
        the = np.linspace(atf1, atf2, 200)
        plt.plot((O2_x + ratio * np.cos(the)), (O2_y + ratio * np.sin(the)), c='b')
        tmplist_x = list((O2_x + ratio * np.cos(the)))
        tmplist_y = list((O2_y + ratio * np.sin(the)))
        if math.sqrt((tmplist_x[0] - Pf[0])**2 + (res_path_y[0] - Pf[1])**2) < math.sqrt((tmplist_y[-1] - Pf[0])**2 + (res_path_y[-1] - Pf[1])**2):
            tmplist_x.reverse()
            tmplist_y.reverse()
        for i in tmplist_x:
            res_path_x.append(i)
        for i in tmplist_y:
            res_path_y.append(i)
        ax = plt.gca()  # 设置等比例
        ax.set_aspect(1)
        plt.title("Dubins Path")
        plt.grid(True)
        plt.show()
        path_dis = 0
        for i in dis_lst:
            path_dis += i
    else:
        # 出发圆弧轨迹`
        the = np.linspace(ati2, ati1, 200)
        res_path_x = list((O1_x + ratio * np.cos(the)))
        res_path_y = list((O1_y + ratio * np.sin(the)))
        if math.sqrt((res_path_x[0] - Pi[0])**2 + (res_path_y[0] - Pi[1])**2) > math.sqrt((res_path_x[-1] - Pi[0])**2 + (res_path_y[-1] - Pi[1])**2):
            res_path_x.reverse()
            res_path_y.reverse()
        # print(len(res_path_x))

        # 直线段
        res_path_x.append(A_x)
        res_path_x.append(B_x)
        res_path_y.append(A_y)
        res_path_y.append(B_y)

        # 到达圆弧轨迹
        the = np.linspace(atf1, atf2, 200)
        tmplist_x = list((O2_x + ratio * np.cos(the)))
        tmplist_y = list((O2_y + ratio * np.sin(the)))
        if math.sqrt((tmplist_x[0] - Pf[0]) ** 2 + (tmplist_y[0] - Pf[1]) ** 2) < math.sqrt((tmplist_x[-1] - Pf[0]) ** 2 + (tmplist_y[-1] - Pf[1]) ** 2):
            tmplist_x.reverse()
            tmplist_y.reverse()
        for i in tmplist_x:
            res_path_x.append(i)
        for i in tmplist_y:
            res_path_y.append(i)
        path_dis = 0
        for i in dis_lst:
            path_dis += i
    # print(len(res_path_x))
    # print(len(res_path_y))
    # print(path_dis)
    return res_path_x, res_path_y, path_dis
    #return ([A_x, A_y], [B_x, B_y], dis_lst)  # 切出点,切入点,每一段的距离

def GetPathLength(point_s=[],point_e=[],ratio=25):
    if len(point_s) < 3 or len(point_e) < 3:
        return
    Pi=[point_s[0], point_s[1]]
    Pf=[point_e[0], point_e[1]]
    alpha = math.radians(point_s[2])
    beta = math.radians(point_e[2])
    trun_types = ["RSR", "LSL", "RSL", "LSR"]
    min_dist = -1
    for i in trun_types:
        tmp_res_path_x, tmp_res_path_y, dis_lst = Dubins_path(Pi=Pi, Pf=Pf, alpha=alpha, beta=beta, ratio=25, tp=i,show_path=False)
        if dis_lst == -1:
            continue
        # print("当前路径长度为", dis_lst)
        if dis_lst == 0:
            continue
        if min_dist == -1 or dis_lst < min_dist:
            min_dist = dis_lst
    # print("最短路径长度为:",min_dist)
    return min_dist